#include "DRRTLocalConnector.h"
#include <iostream>
#include "Conversion.h"
#include "Metrics.h"

bool compare_items (SearchLevel* first, SearchLevel* second)
{
    return first->cost < second->cost;
}

DRRTLocalConnector::DRRTLocalConnector(vector<Prm*>& prms, MultiRobotCollisionDetector& mr_collision_detector, double eps, int top_values_count, double pessimistic_value) :
    m_prm(prms), m_mr_collision_detector(mr_collision_detector), m_eps(eps), m_top_values_count(top_values_count), m_pessimistic_value(pessimistic_value)
{

}
DRRTLocalConnector::~DRRTLocalConnector()
{
    for(vector<SearchLevel*>::iterator iter = m_built.begin(); iter != m_built.end(); ++iter)
    {
        delete *iter;
    }
    m_built.clear();
}

SearchLevel* DRRTLocalConnector::buildItem(Point_d p, SearchLevel* prev, int iterations_left)
{
    SearchLevel* item = new SearchLevel();
    m_built.push_back(item);

    item->level = p;
    item->prev = prev;
    item->iterations_left = iterations_left;
    if (prev == NULL)
    {
        item->costBack = 0;
    }
    else
    {
        item->costBack = prev->costBack + Metrics::distance(prev->level, p);
    }
    item->costNext = Metrics::distance(m_target, p);
    item->cost = item->costBack + m_pessimistic_value * item->costNext;
    return item;
}

list<Point_d> DRRTLocalConnector::connect(vector<Conf> &p_1, vector<Conf> &p_2, int iterations)
{
    m_start = Conversion::to_Point_d(p_1);
    m_target = Conversion::to_Point_d(p_2);
    m_map[m_start] = p_1;
    m_map[m_target] = p_2;
    SearchLevel* start = buildItem(m_start, NULL, iterations);
    m_possible.push_back(start);

    return explore();
}

list<Point_d> DRRTLocalConnector::explore()
{
    double first = m_possible.front()->cost;
    double last = first;
    int counter = 0;
    while (m_possible.size() > 0)
    {
        ++counter;
        SearchLevel* current = m_possible.front();
        last = current->cost;
        m_possible.pop_front();

        if (!visit(current))
        {   //only from unexplored yet possibilities
            continue;
        }
        else if (m_mr_collision_detector.local_planner(m_map[current->level], m_map[m_target], m_eps))
        {   //found target :)
            list<Point_d> items;
            items.push_back(current->level);
            while (current->prev != NULL)
            {
                items.push_back(current->prev->level);
                current = current->prev;
            }
            items.reverse();
            return items;
        }
        else
        {   //explore it (expand)
            if (current->iterations_left > 0)
            {
                list<SearchLevel*> possible_expansions = generate_top_valid_neighbors(current);
                m_possible.insert(m_possible.end(), possible_expansions.cbegin(), possible_expansions.cend());
                m_possible.sort(compare_items);
            }
        }
    }
    std::cout << "[dRRT lc]\tExplored #" << counter << " from " << first << " to " << last << "\n";
    return list<Point_d>();
}

list<SearchLevel*> DRRTLocalConnector::generate_top_valid_neighbors(SearchLevel* p)
{
    list<SearchLevel*> l = generate_all_valid_neighbors(p);

    if (l.size() == 0)
    {   //No items...
        //nothing to do here...
    }
    else if (m_top_values_count == 1)
    {   //No need to sort.. just pick the best one - a bit faster
        list<SearchLevel*>::const_iterator iter = l.cbegin();
        SearchLevel* best_item = *iter;
        for(; iter != l.cend(); ++iter)
        {
            if (compare_items(*iter, best_item))
            {
                best_item = *iter;
            }
        }
        list<SearchLevel*> the_best;
        the_best.push_back(best_item);
        return the_best;
    }
    else
    {   //Sorting is in order :)
        l.sort(compare_items);
        while (m_top_values_count < l.size())
        {
            l.pop_back();
        }
    }
    return l;
}

list<SearchLevel*> DRRTLocalConnector::generate_all_valid_neighbors(SearchLevel* p)
{
    vector<vector<Conf*>> all_directions = generate_all_directions(p);

    list<SearchLevel*> possibles;
    vector<vector<Conf*>::iterator> iters;
    vector<vector<Conf*>::iterator> end_of_iters;
    bool first = true;

    while (first || (iters.size() > 0))
    {
        if (first || (iters.size() < all_directions.size()))
        {
            first = false;
            int index = 0;
            BOOST_FOREACH(vector<Conf*>& one_robot_possibilities, all_directions)
            {
                if (index >= iters.size())
                {
                    iters.push_back(one_robot_possibilities.begin());
                    end_of_iters.push_back(one_robot_possibilities.end());
                }
                ++index;
            }
        }
        Point_d q = buildPoint(iters);
        if (m_mr_collision_detector.local_planner(m_map[p->level], m_map[q], m_eps))
        {
            possibles.push_back(buildItem(q, p, p->iterations_left - 1));
        }
        
        vector<Conf*>::iterator last = iters.back();
        iters.pop_back();
        ++last;
        iters.push_back(last);
        while (last == end_of_iters.back())
        {
            iters.pop_back();
            end_of_iters.pop_back();
            if (iters.size() == 0)
            {
                break;
            }
            last = iters.back();
            iters.pop_back();
            ++last;
            iters.push_back(last);
        }
    }

    return possibles;
}

vector<vector<Conf*>> DRRTLocalConnector::generate_all_directions(SearchLevel* p)
{
    vector<Conf> homes = m_map[p->level];
    vector<Conf>::iterator home_iter = homes.begin();
    vector<vector<Conf*>> all_directions;

    BOOST_FOREACH(Prm* prm, m_prm)
    {
        vector<Conf*> new_samples = prm->get_neighbors(*home_iter);
        ++home_iter;
        all_directions.push_back(new_samples);
    }

    return all_directions;
}

Point_d DRRTLocalConnector::buildPoint(vector<vector<Conf*>::iterator> &iters)
{
    vector<Conf> vec;
    for (vector<vector<Conf*>::iterator>::iterator iter = iters.begin(); iter != iters.end(); ++iter)
    {
        vec.push_back(***iter);
    }
    Point_d p = Conversion::to_Point_d(vec);
    m_map[p] = vec;
    return p;
}

bool DRRTLocalConnector::visit(SearchLevel* p)
{
    BOOST_FOREACH(SearchLevel* other, m_visited)
    {
        if (other->level == p->level)
        {
            return false;
        }
    }
    m_visited.push_back(p);
    return true;
}
